/*
 * @Author: 'daddasd' '3323169544@qq.com'
 * @Date: 2025-07-30 08:55:47
 * @LastEditors: 'daddasd' '3323169544@qq.com'
 * @LastEditTime: 2025-07-30 09:45:09
 * @FilePath: \empty_LP_MSPM0G3507_nortos_ticlang\Hardware\Control\Control.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef _CONTROL_H
#define _CONTROL_H

#include <stdint.h>
//增量式
typedef struct
{
    float Kp;
    float Ki;
    float Kd;

    float error;        // 本次误差 e(k)
    float last_error;   // 上次误差 e(k-1)
    float prev_error;   // 上上次误差 e(k-2)
    float Integral;
    float delta_output; // 增量输出 Δu(k)
    float output;       // 当前控制输出 u(k)
    float output_limit; // 输出限幅
} PID_Inc_TypeDef;


// 位置式PID结构体
typedef struct
{
    float Kp;
    float Ki;
    float Kd;

    float Ek;         // 当前误差
    float Ek_1;       // 上一次误差
    float Integral;   // 积分项

    float Output;     // 输出
    float Output_max; // 输出限幅
} PID_Pos_TypeDef;


// 增量式电机PID初始化
void MotorPID_Init(PID_Inc_TypeDef* pid, float kp, float ki, float kd);
// 增量式电机PID计算
int32_t Speed_Pid_Out(PID_Inc_TypeDef* pid, int32_t setpoint, int32_t measured);

// 位置式PID初始化
void PID_Position_Init(PID_Pos_TypeDef *pid, float kp, float ki, float kd);
// 位置式PID计算
int32_t PID_Position_Calc(PID_Pos_TypeDef *pid, float Target_Value, float Actual_Value);

int32_t wh_Turn_Out(int16_t chazhi, float dir_p, float dir_d);
int  L_speedcontrol(int ECPULSE, int aim_speed, float speed_p, float speed_i);
int  R_speedcontrol(int ECPULSE, int aim_speed, float speed_p, float speed_i);
extern PID_Inc_TypeDef motor_pid;
extern PID_Pos_TypeDef Dir_pid;

#endif
